#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

#include <iostream>

#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <geometry_msgs/msg/pose.hpp>

#include <cv_bridge/cv_bridge.h>

#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include "opencv2/imgproc.hpp"
#include "opencv2/calib3d.hpp"
#include <opencv2/highgui.hpp>
using std::placeholders::_1;

class MinimalSubscriber : public rclcpp::Node
{
  public:
    MinimalSubscriber()
    : Node("minimal_subscriber")
    {
      // subscription_ = this->create_subscription<std_msgs::msg::String>(
      // "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));

      image_subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
      "image_raw", 10, std::bind(&MinimalSubscriber::topic_callback_image, this, _1));

      pointcloud_subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
      "pointcloud_raw", 10, std::bind(&MinimalSubscriber::topic_callback_pointcloud, this, _1));
    }

  private:
    // void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
    // {
    //   RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
    // }

    void topic_callback_image(const sensor_msgs::msg::Image::SharedPtr msg) const
    {
      cv_bridge::CvImagePtr image_ptr;
      image_ptr = cv_bridge::toCvCopy(msg, "bgr8");
      cv::Mat img_ = image_ptr->image;
      RCLCPP_INFO(this->get_logger(), "I get: '%d'*'%d' image", img_.cols, img_.rows);
    }

    void topic_callback_pointcloud(const std_msgs::msg::PointCloud2::SharedPtr msg) const
    { 
      pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloud;
      laserCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
      pcl::fromROSMsg(msg->cloud_corner, *laserCloud);
      RCLCPP_INFO(this->get_logger(), "I get: pointcloud");
    }

    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_subscription_;
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_subscription_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalSubscriber>());
  rclcpp::shutdown();
  return 0;
}